perm filename WRIST.SAI[PNT,HE]5 blob
sn#400422 filedate 1978-11-30 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00008 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00002 00002 IFCR NOT DECLARATION($$PRGID) THENC
C00006 00003 STRING PROCEDURE GETTIM
C00008 00004 ⊃ procedure monitor(command,ppn) will log in a ptyjob with ppn and ex command
C00010 00005 ⊃ MATRIX SOLVERS: DECOMPOSE, SOLVE
C00017 00006 ⊃ MISC ROUTINES: SOLVER, TYPEFORCE
C00019 00007 ⊃ MATRIX INVERSION ROUTINES: INVERT, PINVERSE
C00022 00008 ⊃ START OF PROCEDURE THAT IS CALLED BY POINTY
C00032 ENDMK
C⊗;
IFCR NOT DECLARATION($$PRGID) THENC
ENTRY;
BEGIN "WRIST" ENDC
COMMENT - THIS PROGRAM IS USED TO CALIBRATE THE SCHEINMAN FORCE SENSING
WRIST.;
DEFINE ⊃="COMMENT",CR="'15",LF="'12",CRLF="('15&'12)",FF="'14";
DEFINE NSAMPS=10;
INTEGER I,J,K,DSET;
INTEGER DUM,CHAN,CCHAN,FLAG,ERR;
BOOLEAN TERSE,ASKAGAIN;
BOOLEAN ISCAL,DONTSTOP;
BOOLEAN SATURATED;
STRING COM1;
STRING ANS,MES,LINED;
STRING STOPIT,OUTBUF,OUTBUF2,OUTBUF3;
REAL DX,DY,DZ;
SAFE INTEGER ARRAY PS[1:50];
INTEGER ARRAY READINGS[1:NSAMPS,1:9];
INTEGER ARRAY IBASE[1:9];
REAL ARRAY AVER,CAVER,BASE,SD[1:9];
PRELOAD_WITH 1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 1.0;
REAL ARRAY MPRIME[1:6,1:6];
PRELOAD_WITH
0.0, 0.0, 27.51, 0.0, 0.0, 0.0,
27.51, 0.0, 0.0, 0.0, 55.549, 0.0,
0.0, 27.51, 0.0, -55.549, 0.0, 0.0,
62.79, 0.0, 0.0, 0.0, 214.28, 0.0,
0.0, 62.79, 0.0, -214.28, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 88.248;
OWN REAL ARRAY F[1:6,1:6];
PRELOAD_WITH
-124.0, -7.0, -1.8, 53.0, 115.6, -12.5, -8.0, -65.20,
20.0, 82.0, 134.7, -9.0, 14.0, -83.0, -111.0, 1.00,
-115.0, 8.00, 8.00, 791.0, 119.0, -21.0, -43.0, -789.20,
25.0, 409.0, 58.0, -19.1, 23.0, -398.5, -64.0, 15.1,
3.00, 39.10, 0.00, 35.00, -3.20, 45.00, 2.30, 47.00,
-265.0, 83.0, -138.90, 13.0, -255.20, -73.00, -396.00, -12.00;
OWN REAL ARRAY EPS[1:6,1:8];
REAL ARRAY M[1:6,1:8],MI[1:8,1:6];
EXTERNAL INTEGER PROCEDURE TLKEF6(INTEGER ARRAY READINGS);
REQUIRE "TLKEF6.REL[PNT,HE]" LOAD_MODULE;
PRESET_WITH
"NO CALIBRATION DATA FOR RESOLUTION",
"ROW VALUE MUST BE BETWEEN 1 THRU 6",
"ASKING TO RENAME FILE TO NULL";
INTERNAL STRING ARRAY $WRMSG[1:3];
INTEGER PROCEDURE UGETF(INTEGER CHAN);
BEGIN
define UGETF = '073000;
INTEGER I,CHN; LABEL DOUGTF;
CHN←CHAN;
quick_code;
move '13,CHN;
lsh '13,5;
addi '13,UGETF;
hrlm '13,DOUGTF; ⊃ PREPARE UGETF;
DOUGTF:
I ;
end;
RETURN(I);
END;
INTEGER CVAETAB;
PROCEDURE CVAET;
SETBREAK(CVAETAB←GETBREAK,"@",NULL,"IS");
REQUIRE CVAET INITIALIZATION;
STRING PROCEDURE CVAE(STRING MESS);
BEGIN
STRING S,S1;
INTEGER I;
S1←MESS;
S←NULL;
DO S←S&SCAN(S1,CVAETAB,I)&"E" UNTIL I=0;
RETURN(S[1 TO ∞ - 1]);
END;
STRING PROCEDURE GETTIM;
⊃ DETERMINES THE CURRENT DAY AND TIME, CONVERTS THEM TO ASC STRING
CONSTANTS AND RETURNS THE COMPOSITE STRING.;
BEGIN "GETTIM"
INTEGER DAY,HOUR,T,WID,DIG,YEAR,MON;
PRELOAD_WITH "JAN","FEB","MAR","APR","MAY","JUNE","JULY",
"AUG","SEPT","OCT","NOV","DEC";
OWN STRING ARRAY MONTHS[1:12];
STRING TIME;
⊃ GET THE CURRENT TIME;
GETFORMAT(WID,DIG);
SETFORMAT(-2,0);
TIME←"CURRENT TIME AND DATE: ";
QUICK_CODE
'47540400101;
HLRZ '14,'13;
HRRZ '13,'13;
MOVEM '13,HOUR;
MOVEM '14,DAY;
END;
⊃ COMPUTE AND CONVERT THE TIME OF DAY;
T←HOUR/60;
HOUR←T/60;
T←T-HOUR*60;
TIME←TIME&CVS(HOUR)&":"&CVS(T)&" ";
⊃ COMPUTE AND CONVERT THE DAY OF THE YEAR;
MON←DAY/31;
DAY←(DAY MOD 31)+1;
YEAR←(MON/12)+64;
MON←(MON MOD 12)+1;
TIME←TIME&CVS(DAY)&MONTHS[MON]&CVS(YEAR)&CRLF;
SETFORMAT(WID,DIG);
RETURN(TIME);
END "GETTIM";
⊃ procedure monitor(command,ppn) will log in a ptyjob with ppn and ex command;
SIMPLE INTEGER PROCEDURE LOGIN(STRING PPN(NULL));
BEGIN
STRING S;
external integer _skip_;
INTEGER PTYLINE;
PTYLINE←PTYGET;
ptyline←ptyget;
if _skip_=0 then outstr("couldn't get pty");
IF PPN≠NULL THEN S←PPN ELSE
BEGIN
STRING S1,S2;
S1←CVXSTR(CALL(0,"DSKPPN"))[1 TO 3];
S2←CVXSTR(CALL(0,"DSKPPN"))[4 TO 6];
WHILE S1=" " DO S1←S1[2 TO ∞];
WHILE S2=" " DO S2←S2[2 TO ∞];
S←S1&"."&S2;
END;
ptostr(PTYLINE,"L "&S&CRLF);
S←PTYSTR(PTYLINE,"↑");
S←PTYSTR(PTYLINE,".");
RETURN(PTYLINE);
END;
PROCEDURE MONCOM(INTEGER PTYLINE; STRING COMMAND);
BEGIN
STRING S;
PTOSTR(PTYLINE,COMMAND&CRLF);
S←PTYSTR(PTYLINE,"↑");
S←PTYSTR(PTYLINE,".");
END;
PROCEDURE LOGOUT(INTEGER PTYLINE);
PTYREL(PTYLINE);
PROCEDURE MONITOR(STRING COMMAND,PPN(NULL));
BEGIN
INTEGER PTY;
PTY←LOGIN(PPN);
MONCOM(PTY,COMMAND);
LOGOUT(PTY);
END;
⊃ MATRIX SOLVERS: DECOMPOSE, SOLVE;
PROCEDURE DECOMPOSE(INTEGER N;SAFE REAL ARRAY A,LU);
⊃ Both A and LU are [1:N, 1:N]. Uses global array PS. Computes
triangular matrices L and U and permutation matrix PS so that LU=PA.
Stores (L-I) and U both in LU. The call DECOMPOSE(N,A,A) will
overwrite A with LU. ;
BEGIN "decompose"
INTEGER I, J, K, PIVOTINDEX;
REAL NORMROW, PIVOT, SIZE, BIGGEST, MULT;
SAFE OWN REAL ARRAY R[1:50];
SIMPLE PROCEDURE ILOOP(INTEGER UL;REFERENCE REAL R1,R2);
⊃ Machine-coded for efficiency;
START_CODE
LABEL LP,EU;
MOVE 1,-1('17);
MOVE 2,-2('17);
MOVE 3,-3('17);
SUB 3,K;
JUMPLE 3,EU;
LP: AOJ 1,;
AOJ 2,;
MOVN 4,MULT;
FMPR 4,(1);
FADRM 4,(2);
SOJG 3,LP;
EU: END;
IF N > 50
THEN USERERR(0,1,"DECOMPOSE can't handle a matrix as large as" & CVS(N));
⊃ Initialize PS,LU and R;
FOR I←1 STEP 1 UNTIL N DO
BEGIN
PS[I]←I;
NORMROW←0;
FOR J←1 STEP 1 UNTIL N DO
BEGIN
LU[I,J]←A[I,J];
IF (NORMROW<ABS(LU[I,J])) THEN NORMROW←ABS(LU[I,J]);
END;
IF (NORMROW≠0)
THEN R[I]←1/NORMROW
ELSE BEGIN
R[I]←0;
USERERR(0,1,"Zero row in DECOMPOSE");
END;
END;
⊃ Gaussian elimination with partial pivoting;
FOR K←1 STEP 1 UNTIL N-1 DO
BEGIN "kloop";
BIGGEST ← 0;
FOR I ← K STEP 1 UNTIL N DO
BEGIN
SIZE←ABS(LU[PS[I],K])*R[PS[I]];
IF (BIGGEST<SIZE)
THEN BEGIN
BIGGEST←SIZE;
PIVOTINDEX←I;
END;
END;
IF BIGGEST = 0
THEN BEGIN
USERERR(0,1,"Singular matrix in DECOMPOSE");
DONE "kloop";
END;
IF PIVOTINDEX ≠ K
THEN BEGIN
J←PS[K];
PS[K]←PS[PIVOTINDEX];
PS[PIVOTINDEX]←J;
END;
PIVOT←LU[PS[K],K];
FOR I←K+1 STEP 1 UNTIL N DO
BEGIN
LU[PS[I],K]←MULT←(LU[PS[I],K]/PIVOT);
IF MULT ≠ 0
THEN ILOOP(N,LU[PS[I],K],LU[PS[K],K]);
⊃ The following is the result of the machine code:
FOR J ← K+1 STEP 1 UNTIL N DO
LU[PS[I],J]←LU[PS[I],J]-MULT*LU[PS[K],J];
END;
END "kloop";
IF (LU[PS[N],N]=0)
THEN USERERR(0,1,"Singular matrix in DECOMPOSE");
END "decompose";
SIMPLE PROCEDURE SOLVE(INTEGER N;SAFE REAL ARRAY LU,B,X);
⊃ Arrays LU[1:N,1:N], B[1:N], X[1:N]. Uses global safe integer array
PS. Solves AX=B using LU from DECOMPOSE. ;
BEGIN "solve"
INTEGER I,J;
REAL DOT;
SIMPLE PROCEDURE ILOOP(INTEGER LL,UL;REFERENCE REAL R1,R2);
⊃ Machine-coded for efficiency;
START_CODE
LABEL LP,EU;
MOVE 1,-1('17);
MOVE 2,-2('17);
MOVE 3,-3('17);
SUB 3,-4('17);
SETZ 4,;
JUMPL 3,EU;
LP: MOVE 5,(1);
FMPR 5,(2);
FADR 4,5;
AOJ 1,;
AOJ 2,;
SOJGE 3,LP;
EU: MOVEM 4,DOT;
END;
FOR I ← 1 STEP 1 UNTIL N DO
BEGIN
ILOOP(1,I-1,LU[PS[I],1],X[1]);
⊃ Has this effect:
DOT←0
FOR J←1 STEP 1 UNTIL I-1 DO
DOT←DOT+LU[PS[I],J]*X[J];
X[I]←B[PS[I]]-DOT;
END;
X[N] ← X[N] / LU[PS[N],N];
FOR I ← N-1 STEP -1 UNTIL 1 DO
BEGIN ⊃ RF: I changed loop upper index from N, to avoid
subscript errors;
ILOOP(I+1,N,LU[PS[I],I+1],X[I+1]);
⊃ Has this effect:
DOT←0
FOR J←I+1 STEP 1 UNTIL N DO
DOT←DOT+LU[PS[I],J]*X[J];
X[I]←(X[I]-DOT)/LU[PS[I],I];
END;
END "solve";
⊃ MISC ROUTINES: SOLVER, TYPEFORCE;
PROCEDURE SOLVER(REAL ARRAY MI,EPS,F);
BEGIN "SOLVER"
INTEGER I,J,K;
REAL ARRAY LU[1:6,1:6],E[1:6],M[1:6];
⊃ TRIANGULARIZE THE FORCE MATRIX;
DECOMPOSE(6,F,LU);
⊃ COPY THE SIX READINGS FOR EACH GAGE AND SOLVE FOR A
ROW OF THE INVERSE CALIBRATION MATRIX. REPEAT FOR
ALL EIGHT STRAIN GAGE PAIRS.;
FOR I ← 1 STEP 1 UNTIL 8 DO
BEGIN "SOLOOP"
FOR J ← 1 STEP 1 UNTIL 6 DO E[J]←EPS[J,I];
SOLVE(6,LU,E,M);
FOR J ← 1 STEP 1 UNTIL 6 DO MI[I,J]←M[J];
END "SOLOOP";
END "SOLVER";
PROCEDURE TYPEFORCE(REAL ARRAY F);
BEGIN "TYPEFORCE"
REAL MAG;
OUTSTR(CRLF&"THE RESULTING FORCE VECTOR IS ("&CVF(F[1])&
","&CVF(F[2])&","&CVF(F[3])&")"&CRLF&
"THE RESULTING MOMENT VECTOR IS("&CVF(F[4])&
","&CVF(F[5])&","&CVF(F[6])&")"&CRLF);
MAG← ( F[1]↑2 + F[2]↑2 + F[3]↑2 )↑0.5;
OUTSTR("THE MAGNITUDE OF THE FORCE IS "&CVF(MAG)&CRLF);
END "TYPEFORCE";
⊃ MATRIX INVERSION ROUTINES: INVERT, PINVERSE;
PROCEDURE INVERT (INTEGER N; REAL ARRAY A );
⊃ COMPUTES THE INVERSE OF THE NxN MATRIX "A" AND RETURNS THE INVERTED
MATRIX IN "A". THE PROCEDURES "SOLVE" AND "DECOMPOSE" ARE USED TO
COMPUTE THE INDIVIDUAL ROWS OF THE INVERSE MATRIX.;
BEGIN "INVERT"
INTEGER I,J;
REAL ARRAY LU[1:N,1:N],IDENT[1:N],X[1:N];
⊃ COPY THE ARRAY AND TRIANGULARIZE IT;
ARRTRAN(LU,A);
DECOMPOSE(N,LU,LU);
⊃ COMPUTE THE ROWS OF THE INVERSE ONE BY ONE;
FOR I ← 2 STEP 1 UNTIL N DO IDENT[I]←0.0;
FOR I ← 1 STEP 1 UNTIL N DO
BEGIN "INVLOOP"
IDENT[I]←1.0;
SOLVE(N,LU,IDENT,X);
FOR J ← 1 STEP 1 UNTIL N DO A[J,I]←X[J];
IDENT[I]←0.0;
END "INVLOOP";
END "INVERT";
PROCEDURE PINVERSE(REAL ARRAY M,MI);
⊃ COMPUTES THE PSUEDO INVERSE OF A NON-SQUARE 6x8 MATRIX, MI, AND
RETURNS THE INVERTED 8x6 MATRIX IN M. THE EQUATION IMPLEMENTED BY
THIS ROUTINE IS AS FOLLOWS:
T -1 T
M ← ( MI * MI ) * MI
WHERE THE "*" DENOTES MATRIX MULTIPLICATION;
BEGIN "PINVERSE"
REAL ARRAY A[1:6,1:6];
REAL STOTAL;
INTEGER I,J,K;
⊃ COMPUTE THE PRODUCT OF MI AND ITS TRANSPOSE;
FOR I ← 1 STEP 1 UNTIL 6 DO
FOR J ← 1 STEP 1 UNTIL 6 DO
BEGIN "PMULT"
STOTAL←0.0;
FOR K ← 1 STEP 1 UNTIL 8 DO
STOTAL←STOTAL+MI[K,I]*MI[K,J];
A[I,J]←STOTAL;
END "PMULT";
⊃ INVERT THE PRODUCT AND MULTIPLY BY THE TRANSPOSE AGAIN;
INVERT(6,A);
FOR I ← 1 STEP 1 UNTIL 6 DO
FOR J ← 1 STEP 1 UNTIL 8 DO
BEGIN "FMULT"
STOTAL←0.0;
FOR K ←1 STEP 1 UNTIL 6 DO
STOTAL←STOTAL+A[I,K]*MI[J,K];
M[I,J]←STOTAL;
END "FMULT";
END "PINVERSE";
⊃ START OF PROCEDURE THAT IS CALLED BY POINTY;
STRING FILE;
PROCEDURE INIT;
FILE←"FORCAL.CAL";
REQUIRE INIT INITIALIZATION;
INTERNAL INTEGER PROCEDURE RWRIST(STRING MODE; INTEGER WHICHROW(0);STRING FILENAME(NULL));
BEGIN INTEGER ERRCODE; ERRCODE←0;
IF EQU(MODE,"INIT")
THEN
BEGIN "INIT"
DX← DY← DZ ← 0.0;
TERSE←TRUE;
LINED←""; COM1←"";
⊃ READ IN THE CALIBRATION TABLE IF IT EXISTS, AND TYPE AN APPROPRIATE
MESSAGE.;
CCHAN←GETCHAN;
OPEN(CCHAN,"DSK",0,2,0,DUM,DUM,DUM);
LOOKUP(CCHAN,FILE,FLAG);
IF FLAG=0 THEN BEGIN
FOR I ← 1 STEP 1 UNTIL 6 DO
FOR J ←1 STEP 1 UNTIL 8 DO M[I,J]←REALIN(CCHAN);
OUTSTR("CALIBRATION TABLE READ FROM DISK"&CRLF);
ISCAL←TRUE;
END ELSE BEGIN
OUTSTR("NO CALIBRATION DATA FOUND ON DISK"&CRLF);
ISCAL←FALSE;
END;
RELEASE(CCHAN);
END "INIT"
ELSE IF EQU(MODE,"READ") THEN
BEGIN "READ"
INTEGER W,D;
SATURATED←FALSE;
DO ERR←TLKEF6(READINGS) UNTIL ERR=0;
⊃ COMPUTE STATISTICS FOR READINGS.;
FOR I←1 STEP 1 UNTIL 9 DO
BEGIN
AVER[I]←0.0;
SD[I]←0.0;
END;
FOR I←1 STEP 1 UNTIL NSAMPS DO
FOR J←1 STEP 1 UNTIL 9 DO
BEGIN
IF (J<9)∧(READINGS[I,J]<-1750.0 ∨ READINGS[I,J]>2000.0) THEN
SATURATED←TRUE;
AVER[J]←AVER[J]+READINGS[I,J];
SD[J]←SD[J]+READINGS[I,J]↑2;
END;
FOR I←9 STEP -1 UNTIL 1 DO
BEGIN
AVER[I]←AVER[I]/NSAMPS;
CAVER[I]←AVER[I]-BASE[I]-AVER[9]+BASE[9];
SD[I]←((SD[I]-NSAMPS*AVER[I]↑2)/(NSAMPS-1))↑0.5;
END;
GETFORMAT(W,D);
SETFORMAT(9,2);
IF SATURATED THEN OUTSTR(CRLF&"WRIST SENSOR READING OUT OF RANGE"&CRLF);
OUTBUF←GETTIM&
"Strain Gage Readings: Mean, Corrected Mean, Standard Dev."&
CRLF;
OUTBUF2←OUTBUF3←"";
FOR I ← 1 STEP 1 UNTIL 9 DO
BEGIN
OUTBUF←OUTBUF&CVF(AVER[I]);
OUTBUF2←OUTBUF2&CVF(CAVER[I]);
OUTBUF3←OUTBUF3&CVF(SD[I]);
END;
OUTBUF←OUTBUF&CRLF&OUTBUF2&CRLF&OUTBUF3&CRLF&CRLF;
SETFORMAT(W,D);
END "READ"
ELSE IF EQU(MODE,"BASE") THEN
BEGIN "BASE"
⊃ USER WANTS TO SET NEW DATA OFFSET;
FOR I←1 STEP 1 UNTIL 9 DO
BEGIN
BASE[I]←AVER[I];
IBASE[I]←READINGS[1,I];
END;
END "BASE"
⊃ FORCE AND MOMENT COMPUTATION;
ELSE IF EQU(MODE,"RESOLVE") THEN
BEGIN "RESOLVE"
IF ¬ISCAL THEN ERRCODE←1
ELSE BEGIN
REAL ARRAY F[1:6],FPRIME[1:6];
SETFORMAT(8,2);
FOR I←1 STEP 1 UNTIL 6 DO
BEGIN
F[I]←0.0;
FOR J←1 STEP 1 UNTIL 8 DO
F[I]←F[I]+M[I,J]*(READINGS[1,J]-IBASE[J]
-READINGS[1,9]+IBASE[9]);
END;
TYPEFORCE(F);
FOR I←1 STEP 1 UNTIL 6 DO
BEGIN
FPRIME[I]←0.0;
FOR J←1 STEP 1 UNTIL 6 DO
FPRIME[I]←FPRIME[I]+MPRIME[I,J]*F[J];
END;
OUTSTR(CRLF&"FORCE/MOMENTS RECOMPUTED AT ("&CVF(DX)&
","&CVF(DY)&","&CVF(DZ)&")"&CRLF);
TYPEFORCE(FPRIME);
ASKAGAIN←FALSE;
END
END "RESOLVE"
⊃ USE DATA FOR FORCE CALIBRATION, PRINT CURRENT DATA;
ELSE IF EQU(MODE,"CALIB") THEN
BEGIN "CALIB"
⊃ REPLACE OLD DATA WITH NEW;
IF 1≤WHICHROW≤ 6 THEN
FOR I ← 1 STEP 1 UNTIL 8 DO
EPS[WHICHROW,I]←CAVER[I]
ELSE ERRCODE←2;
END "CALIB"
⊃ ASK IF THE CALIBRATION MATRIX IS TO BE COMPUTED;
ELSE IF EQU(MODE,"COMPUTE") THEN
BEGIN "COMPUTE"
SOLVER(MI,EPS,F);
PINVERSE(M,MI);
ISCAL←TRUE;
END "COMPUTE"
⊃ SAVE NEW CALIBRATION ON THE DISK?;
ELSE IF EQU(MODE,"SAVECALIB") THEN
BEGIN "SAVECALIB"
CHAN←GETCHAN;
OPEN(CHAN,"DSK",0,0,2,120,DUM,DUM);
ENTER(CHAN,"FORCAL.CAL",DUM);
SETFORMAT(15,7);
FOR I←1 STEP 1 UNTIL 6 DO
BEGIN "PLINE"
MES←"";
FOR J ← 1 STEP 1 UNTIL 4 DO
MES←MES&CVE(M[I,J])&" ";
MES←MES&CRLF;
FOR J ← 5 STEP 1 UNTIL 8 DO
MES←MES&CVE(M[I,J])&" ";
MES←MES&CRLF;
OUT(CHAN,MES&CRLF);
END "PLINE";
OUT(CHAN,CRLF&CRLF&"CALIBRATION MATRIX: "&GETTIM);
RELEASE(CHAN);
END "SAVECALIB"
⊃ SAVE NEW CALIBRATION ON THE DISK?(SAVE TRANSPOSE,SUITABLE FOR PALX);
ELSE IF EQU(MODE,"SAVECALIBPAL") THEN
BEGIN "SAVECALIBPAL"
CHAN←GETCHAN;
OPEN(CHAN,"DSK",0,0,2,120,DUM,DUM);
ENTER(CHAN,"FORCAL.PAL",DUM);
MES←
".TITLE WRIST SENSOR CALIBRATION DATA TRANSPOSED
.=2000 ;START ADDRESS OF ARM DATA
.OFFSET -320000 ;PUT IT IN THE DATA AREA
";
OUT(CHAN,MES);
OUT(CHAN,CRLF&CRLF&";CALIBRATION MATRIX: "&GETTIM);
SETFORMAT(15,7);
FOR J←1 STEP 1 UNTIL 8 DO
BEGIN "PLINE"
MES←".FLT2 ";
FOR I ← 1 STEP 1 UNTIL 6 DO
MES←MES&CVE(M[I,J])&",";
MES←CVAE(MES);
OUT(CHAN,MES[1 to ∞ -1]&CRLF);
END "PLINE";
OUT(CHAN,".END"&CRLF);
RELEASE(CHAN);
END "SAVECALIBPAL"
ELSE IF EQU(MODE,"RENAMEFILE") THEN
BEGIN "RENAMEFILE"
IF FILENAME =NULL THEN ERRCODE←3;
FILE←FILENAME;
END "RENAMEFILE"
ELSE IF EQU(MODE,"SAVERAWDATA") THEN
BEGIN "SAVERAWDATA"
INTEGER CHAN;
CHAN←getchan;
OPEN(CHAN,"DSK",0,2,2,DUM,DUM,DUM);
LOOKUP(CHAN,"FORCAL.DAT",DUM);
ENTER(CHAN,"FORCAL.DAT",DUM);
UGETF(CHAN);
COMMENT FILENAME ACTUALLY IS THE COMMENT STRING;
OUT(CHAN,FILENAME&CRLF&OUTBUF&CRLF&FF);
RELEASE(CHAN);
END "SAVERAWDATA"
ELSE IF EQU(MODE,"DISPRAWDATA") THEN
OUTSTR(CRLF&OUTBUF&CRLF)
ELSE IF EQU(MODE,"COMPILEPALFILE") THEN
MONITOR("COMPILE FORCAL.PAL")
ELSE IF EQU(MODE,"COMPILEPALFILEONTOALHE") THEN
BEGIN
MONITOR("COMPILE FORCAL.PAL");
MONITOR("REN FORCAL.BIN[AL,HE]←FORCAL.BIN/Q");
END
ELSE OUTSTR(MODE& "is an unrecognized mode, choose one of the following"&crlf&
" INIT - initialize"&crlF&
" READ - read force sensors"&crlf&
" BASE - base readings"&CRLF&
" RESOLVE - resove forces and moments"&CRLF&
" CALIB - calibrate"&CRLF&
" SAVECALIB - save calibration data set on disk file"&crlf&
" SAVECALIBPAL-save palx version of transposed calibration data"&crlf&
" SAVERAWDATA - save wrist readings"&crlf&
" COMPILEPALFILE - compile FORCAL.PAL on current ppn"&crlf&
" COMPILEPALFILEONTOALHE - compile FORCAL.PAL onto AL,HE"&crlf&
" DISPRAWDATA - display wrist raw data"&crlf&
" RENAMEFILE - change force calibration data file"&CRLF);
RETURN(ERRCODE);
END;
END "WRIST"